#include <ros/ros.h>
#include <iostream>
#include "./../../kinematics/Kine.h"
#include <birl_manipulator/ForwardKine.h>
#include <birl_manipulator/TargetPose.h>

int main(int argc,char* argv[])
{
    std::string user_answer;
    ros::init(argc,argv,"manipulator_fk_client");
    if(argc!=6)
    {
        ROS_INFO("usage: fk_client angle1 angle2 angle3 angle4 angle5");
        return 1;
    }
    

    ros::NodeHandle node_handle;
    // client 用于发送关节角度给fk_server计算运动学正解
    ros::ServiceClient client= node_handle.serviceClient<birl_manipulator::ForwardKine>("ForwardKine");
    // move_robot_client 用于发送关节角度给manipulator_setup 使机器人运动起来
    ros::ServiceClient move_robot_client = node_handle.serviceClient<birl_manipulator::TargetPose>("manipulator_position_service");


    birl_manipulator::ForwardKine srv;
    srv.request.current_joint_state.clear();
    for(int i=0;i<5;i++){
    srv.request.current_joint_state.push_back(atof(argv[i+1]));
    }
    if(client.call(srv)){
        // 当求解标志为True时才进行正解结果的显示以及关节角度的发送
       if(srv.response.ifGetSolve){
           ROS_INFO("Get the Fk solution: ");
           ROS_INFO_STREAM(" "<<srv.response.descartes_pos_state[0]<<" "
                              <<srv.response.descartes_pos_state[1]<<" "
                              <<srv.response.descartes_pos_state[2]<<" "
                              <<srv.response.descartes_pos_state[3]<<" "
                              <<srv.response.descartes_pos_state[4]<<" "
                              <<srv.response.descartes_pos_state[5]);

            // 命令行交互，决定是否按照输入的角度使机器人运动起来
           std::cout<<"Move the Robot or not?(Yes or No):";
           std::cout<<(user_answer.compare("Yes"));
           if(user_answer.compare("Yes")==0){
               birl_manipulator::TargetPose move_robot_srv;
               for(int i=0;i<5;i++){
                   move_robot_srv.request.target_pose[i]=srv.request.current_joint_state[i];
                }
                move_robot_client.call(move_robot_srv); 
            }
            else
            {
                return 0;
            }
       } 
    }
    else{
        ROS_ERROR("Failed to call service fk_server");
        return 1;
    }

    return 0;

    
}